﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using BaseDll;
using Communicate;
using SerialDict;
using MotionIoLib;
using System.Threading;
using System.ComponentModel;
using System.Net;
using System.Net.Sockets;
using CommonTools;
using ABB.Robotics.Controllers;
using ABB.Robotics.Controllers.Discovery;
using ABB.Robotics.Controllers.RapidDomain;
using ABB.Robotics.Controllers.IOSystemDomain;
using ABB.Robotics.Controllers.EventLogDomain;

namespace MotionIoLib
{
    public class SixRobot_ABB : BaseSixRobot
    {
        public static Controller controller = null;
        private NetworkScanner Scanner = null;
        public ABB.Robotics.Controllers.EventLogDomain.EventLog eve_Log = null;
        string str_IP地址 = "192.168.6.40";//机器人ip  192.168.6.40
        int n_Port = 5000;
        string str_ClientIP = "192.168.6.69";//机器人ip  192.168.6.69
        string str_ServoInput = "";
        object obj_Lock = new object();
        public SixRobot_ABB(string strName, params object[] RobotParam) : base(strName, RobotParam)
        {
            this.strName = strName;
            str_IP地址 = RobotParam[0].ToString();
            n_Port = int.Parse(RobotParam[1].ToString());
            str_ClientIP = RobotParam[2].ToString();
            str_ServoInput = RobotParam[3].ToString();
        }
        public Queue<string> m_messageQueue = new Queue<string>();
        private TcpLink link = null;
        bool b_IsConnect = false;

        public override bool Open()
        {
            if (Scanner == null)
            {
                Scanner = new NetworkScanner();
            }
            //对网络进行扫描
            Scanner.Scan();
            //网络上所有机器人的信息返回给ControllerInfoCollection类型的数据ABB_Collection
            ControllerInfoCollection ABB_Collection = Scanner.Controllers;
            foreach (ControllerInfo info in ABB_Collection)
            {
                //遍历所有机器人的信息 并按ListView_ABB的规格显示各条目
                ListViewItem ABB_Items = new ListViewItem(info.SystemName);
                //对ABB_Item逐个添加信息，均转化为字符串
                ABB_Items.SubItems.Add(info.IPAddress.ToString());
                ABB_Items.SubItems.Add(info.Version.ToString());
                ABB_Items.SubItems.Add(info.IsVirtual.ToString());
                ABB_Items.SubItems.Add(info.ControllerName.ToString());
                ABB_Items.Tag = info;

                if (info.Availability == Availability.Available)
                {
                    //如果controller不为null，先登出并Dispose
                    if (controller != null)
                    {
                        controller.Logoff();
                        controller.Dispose();
                        controller = null;
                    }
                    //controller = Controller.Connect(info.SystemId, ConnectionType.Standalone);
                    controller = ControllerFactory.CreateFrom(info);
                    //使用默认用户名登录
                    controller.Logon(UserInfo.DefaultUser);
                    controller.EventLog.MessageWritten += new EventHandler<MessageWrittenEventArgs>(msg_WritenChanged);

                    ReConnect();
                    return true;
                }
            }
            return false;

        }
        string strErrorInfo = "NoError";
        private void msg_WritenChanged(object sender, MessageWrittenEventArgs e)
        {
            if(e.Message.Type==ABB.Robotics.Controllers.EventLogDomain.EventLogEntryType.Error)
               strErrorInfo = e.Message.Title;
        }

        public override bool ReConnect()
        {
            try
            {
                if (controller != null)
                {
                    
                    if (controller.OperatingMode == ControllerOperatingMode.Auto)
                    {
                        using (Mastership m = Mastership.Request(controller.Rapid))
                        {
                            controller.Rapid.Stop(StopMode.Immediate);
                        }

                        if (SetServo(true) != 0)
                            return false;
                        DateTime time = DateTime.Now;
                        ///程序指针复原？？？？？？？？
                        ABB.Robotics.Controllers.RapidDomain.Task[] tasks = controller.Rapid.GetTasks();
                        double d_Time = (DateTime.Now - time).TotalSeconds;
                        using (Mastership m = Mastership.Request(controller))
                        {
                            try
                            {
                                tasks[0].ResetProgramPointer();

                            }
                            catch (Exception)
                            {

                            }
                        }
                        Thread.Sleep(1000);
                        
                        using (Mastership m = Mastership.Request(controller))
                        {

                            StartResult result = controller.Rapid.Start();
                            Thread.Sleep(1000);
                            if (result == StartResult.Ok)
                            {
                                link = new TcpLink(0, "ABB机器人", "192.168.125.1", 8080, 2000, "");
                                link.RecvStringMessageEvent += (object sender, AsyTcpSocketEventArgs e) =>
                                {
                                    InMessageQueue(e.Message);
                                };
                                link.SocketErrorEvent += (object sender, AsyTcpSocketEventArgs e) =>
                                {
                                    return;
                                };
                                if (link.Open())
                                {
                                    b_IsConnect = true;
                                    return true;
                                }
                                else
                                    return false;
                            }
                            else
                                return false;
                        }
                    }
                    else
                    {
                        MessageBox.Show("若要自动运行，请先将机器人切换到自动运行模式");
                        return false;
                    }
                }
                else
                {
                    MessageBox.Show("请先连接ABB机器人");
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message.ToString());
                return false;
            }
        }
        public void clearMessageQueue()
        {
            m_messageQueue.Clear();
        }
        public int OutMessageQueue(out string mes)
        {
            mes = "";
            if (m_messageQueue.Count > 0)
            {
                mes = m_messageQueue.Dequeue();
                return 0;
            }
            else
                return -1;
        }
        public void InMessageQueue(string mes)
        {
            m_messageQueue.Enqueue(mes);
        }
        public void Send(string msg)
        {
            if (link != null)
            {
                link.WriteString(msg + "\r\n");
            }

        }

        private int Function_SendCmd(string strCmd, ref string strResult, int n_TimeOut = 2000)
        {
            lock (obj_Lock)
            {
                clearMessageQueue();
                this.Send(strCmd);
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Restart();
                while (true)
                {
                    Thread.Sleep(10);
                    string str_Recevice = "";
                    if (OutMessageQueue(out str_Recevice) == 0)
                    {
                        strResult = str_Recevice.Clone().ToString();
                        return 0;
                    }
                    else if (stopwatch.ElapsedMilliseconds > n_TimeOut)
                    {
                        return -4;
                    }
                }
            }
        }
        public override bool ReadDigitalInPort(int n_Index)
        {
            try
            {
                Signal DI_X = controller.IOSystem.GetSignal($"DI_{n_Index}");
                string str_DIValue = DI_X.Value.ToString();
                if (str_DIValue == "1")
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}"); 
                return false;
            }
        }
        //读取输出信号
        public override bool ReadDigitalOutPort(int n_Index)
        {
            try
            {
                Signal DO_X = controller.IOSystem.GetSignal($"DO_{n_Index}");
                string str_DIValue = DO_X.Value.ToString();
                if (str_DIValue == "1")
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}");
                return false;
            }
        }
        //设置输出信号
        public override bool SetDigitalOutputPort(int n_Index, bool b_On)
        {
            try
            {
                //？？？？？？
                Signal DO_X = controller.IOSystem.GetSignal($"DO_{n_Index}");
                double d_OldValue = DO_X.Value;
                DO_X.Value = Convert.ToSingle(b_On);
                if (DO_X.Value != d_OldValue)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}");
                return false;
            }
        }
        public override SixRobotJoint GetCurrentPos_Joint()
        {
            try
            {
                SixRobotJoint PointWorldObject = new SixRobotJoint(0, 0, 0, 0, 0, 0);
                if (controller != null)
                {
                    try
                    {
                        JointTarget ABB_Joint = controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
                        PointWorldObject.A1 = ABB_Joint.RobAx.Rax_1;
                        PointWorldObject.A2 = ABB_Joint.RobAx.Rax_2;
                        PointWorldObject.A3 = ABB_Joint.RobAx.Rax_3;
                        PointWorldObject.A4 = ABB_Joint.RobAx.Rax_4;
                        PointWorldObject.A5 = ABB_Joint.RobAx.Rax_5;
                        PointWorldObject.A6 = ABB_Joint.RobAx.Rax_6;
                    }
                    catch (Exception ex)
                    {
                        PointWorldObject.A1 = 0;
                        PointWorldObject.A2 = 0;
                        PointWorldObject.A3 = 0;
                        PointWorldObject.A4 = 0;
                        PointWorldObject.A5 = 0;
                        PointWorldObject.A6 = 0;
                        MessageBox.Show(ex.Message.ToString());
                    }
                }
                else
                {
                    PointWorldObject.A1 = 0;
                    PointWorldObject.A2 = 0;
                    PointWorldObject.A3 = 0;
                    PointWorldObject.A4 = 0;
                    PointWorldObject.A5 = 0;
                    PointWorldObject.A6 = 0;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}");
                return new SixRobotJoint(0, 0, 0, 0, 0, 0); ;
            }
        }
        //获取当前位置---世界坐标系
        public override SixRobotPoint GetCurrentPos_World()
        {
            try
            {
                SixRobotPoint PointWorldObject = new SixRobotPoint(0, 0, 0, 0, 0, 0);
                double rx = 0, ry = 0, rz = 0;
                if (controller != null)
                {
                    try
                    {
                        RobTarget ABB_World = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(ABB.Robotics.Controllers.MotionDomain.CoordinateSystemType.World);
                        ABB_World.Rot.ToEulerAngles(out rx, out ry, out rz);
                        PointWorldObject.X = ABB_World.Trans.X;
                        PointWorldObject.Y = ABB_World.Trans.Y;
                        PointWorldObject.Z = ABB_World.Trans.Z;
                        PointWorldObject.W = rx;
                        PointWorldObject.P = ry;
                        PointWorldObject.R = rz;
                    }
                    catch (Exception ex)
                    {
                        PointWorldObject.X = 0;
                        PointWorldObject.Y = 0;
                        PointWorldObject.Z = 0;
                        PointWorldObject.W = 0;
                        PointWorldObject.P = 0;
                        PointWorldObject.R = 0;
                        MessageBox.Show(ex.Message.ToString());
                    }
                }
                else
                {
                    PointWorldObject.X = 0;
                    PointWorldObject.Y = 0;
                    PointWorldObject.Z = 0;
                    PointWorldObject.W = 0;
                    PointWorldObject.P = 0;
                    PointWorldObject.R = 0;
                }

                return PointWorldObject;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"埃斯顿机器人运行错误：{ex.Message}");
                return new SixRobotPoint(0, 0, 0, 0, 0, 0); ;
            }
        }
        //直线运动
        public override bool MoveLine(SixRobotPoint Point, bool bmamual, int Speed = 1000, int BlendValue = 100)
        {
            StationCheck(bmamual);
            string Commond = $"PLCsend,1,{Point.X.ToString("0.000")},{Point.Y.ToString("0.000")},{Point.Z.ToString("0.000")},{Point.W.ToString("0.000")},{Point.P.ToString("0.000")},{Point.R.ToString("0.000")}";
            int n_Time = 0;
        Retry:
            int n_Result = 0;
            string strMeg = "";
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, Commond);
            n_Result = Function_SendCmd(Commond, ref strMeg, 6000);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, strMeg);
            if (n_Result != 0)
            {
                n_Time++;
                if (n_Time < 3)
                    goto Retry;
            }
            if (n_Result == 0)
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    Thread.Sleep(10);
                    SixRobotPoint point = GetCurrentPos_World();
                    b_Result = true;
                    b_Result &= Math.Abs(point.X - Point.X) < 0.5;
                    b_Result &= Math.Abs(point.Y - Point.Y) < 0.5;
                    b_Result &= Math.Abs(point.Z - Point.Z) < 0.5;
                    if (b_Result)
                    {
                        stopwatch.Stop();
                        break;
                    }
                    if (stopwatch.ElapsedMilliseconds > 60000)
                        break;
                }
                if (!b_Result)
                {
                    string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strErrorInfo };
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                    throw (new Exception("ABB机器人运动到位失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.X.ToString(), Point.Y.ToString(), Point.Z.ToString(), Point.W.ToString(), Point.P.ToString(), Point.R.ToString(), strErrorInfo };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveL超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveL失败");
                throw (new Exception("ABB机器人运动指令发送失败"));
            }
        }
        //关节运动
        public override bool MoveJog(SixRobotJoint Point,bool bmamual, int Speed = 100, int BlendValue = 100)
        {
            StationCheck(bmamual);
            string Commond = $"PLCsend,2,{Point.A1.ToString("0.000")},{Point.A2.ToString("0.000")},{Point.A3.ToString("0.000")},{Point.A4.ToString("0.000")},{Point.A5.ToString("0.000")},{Point.A6.ToString("0.000")}";
            int n_Time = 0;
        Retry:
            int n_Result = 0;
            string strMeg = "";
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, Commond);
            n_Result = Function_SendCmd(Commond, ref strMeg, 6000);
            BaseDll.DeviceDataCollect.m_DeviceData.Function_MachineLog(strName, strMeg);
            if (n_Result != 0)
            {
                n_Time++;
                if (n_Time < 3)
                    goto Retry;
            }
            if (n_Result == 0)
            {
                bool b_Result = true;
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                while (true)
                {
                    Thread.Sleep(10);
                    SixRobotJoint point = GetCurrentPos_Joint();
                    b_Result = true;
                    b_Result &= Math.Abs(point.A1 - Point.A1) < 1;
                    b_Result &= Math.Abs(point.A2 - Point.A2) < 1;
                    b_Result &= Math.Abs(point.A3 - Point.A3) < 1;
                    b_Result &= Math.Abs(point.A4 - Point.A4) < 1;
                    b_Result &= Math.Abs(point.A5 - Point.A5) < 1;
                    b_Result &= Math.Abs(point.A6 - Point.A6) < 1;
                    if (b_Result)
                        break;
                    if (stopwatch.ElapsedMilliseconds > 60000)
                        break;
                }
                if (!b_Result)
                {
                    string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strErrorInfo };
                    AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                    throw (new Exception("机器人运动失败"));
                }
                return true;
            }
            else
            {
                string[] list_Input = new string[] { Point.A1.ToString(), Point.A2.ToString(), Point.A3.ToString(), Point.A4.ToString(), Point.A5.ToString(), Point.A6.ToString(), strErrorInfo };
                AlarmMgr.GetIntance().WarnWithDlg((int)SixRobotAlarm.六轴机器人MoveJ超时, new string[] { "退出" }, list_Input, "Eston六轴机器人MoveJ失败");
                throw (new Exception("ABB机器人运动指令发送失败"));
            }
        }

        private void StationCheck(bool bmanual)
        {
            if (GlobalVariable.g_StationState == StationState.StationStatePause)
            {
                while (true)
                {
                    Thread.Sleep(10);
                    if (GlobalVariable.g_StationState == StationState.StationStateRun)
                        break;
                    else if (GlobalVariable.g_StationState == StationState.StationStatePause)
                        continue;
                    else
                        throw new Exception("自动运行停止");
                }
            }
            if (GlobalVariable.g_StationState == StationState.StationStateRun && bmanual)
            {
                throw new Exception("自动运行的情况下，禁止bmanual=true的运动指令");
            }
            if (GlobalVariable.g_StationState != StationState.StationStateRun && !bmanual)
            {
                throw new Exception("手动运行的情况下，禁止bmanual=false的运动指令");
            }

        }

        public override int SetServo(bool b_Statue)
        {
            try
            {
                if (controller != null)
                {
                    if (controller.OperatingMode == ControllerOperatingMode.Auto)
                    {
                        if(controller.State== ControllerState.MotorsOn && !b_Statue)
                            controller.State =  ControllerState.MotorsOff;
                        if (controller.State == ControllerState.MotorsOff && b_Statue)
                            controller.State = ControllerState.MotorsOn;
                        return 0;
                    }
                    else
                    {
                        MessageBox.Show("请切换到自动模式");
                        return -1;
                    }
                }
                else
                {
                    MessageBox.Show("请先连接ABB机器人");
                    return -1;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}");
                return -1;
            }
        }
        public override bool ReadServoStatue()
        {
            try
            {
                if (controller != null)
                {
                    return controller.State == ControllerState.MotorsOn;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}"); 
                return false;
            }
        }
        public override int ResetAlarm()
        {
            return 0;
        }
        public override int SetRunSpeed(int Speed)
        {
            try
            {
                if (controller!=null)
                {
                    using (Mastership m = Mastership.Request(controller))
                    {
                        controller.MotionSystem.SpeedRatio = Speed;
                        return 0;
                    }
                }
                else
                {
                    MessageBox.Show("请先连接ABB机器人");
                    return -1;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}"); 
                return -1;
            }
        }
        public override int Stop()
        {
            try
            {
                if (controller != null)
                {
                    using (Mastership m = Mastership.Request(controller))
                    {
                        controller.Rapid.Stop(StopMode.Immediate);
                    }
                    return 0;
                }
                else
                {
                    MessageBox.Show("请先连接ABB机器人");
                    return -1;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}"); 
                return -1;
            }
        }
        public override bool IsConnect()
        {
            try
            {
                return b_IsConnect;
            }
            catch (Exception ex)
            {
                MessageBox.Show($"ABB机器人运行错误：{ex.Message}"); 
                return false;
            }
        }


    }
}